/*
 *  Created by juangeng on 21/08/19.
 *  rl_c1_driver 机器人底盘控制与tcp/ip连接
 *  ________________________________
 *  chassis
 *  通过usb串口与机器人底盘通讯
 *  上位机→底盘： 发送控制指令（前进线速度-m/s；转弯角速度-rad/s；具体数据格式见通讯协议）
 *
 *  上位机←底盘： 接收机器人状态（里程计、底盘电池状态、舵机状态等；具体数据格式见通讯协议）
 *
 * */

#include "chassis_driver.h"

#define VEL_COMMAND_LEN 19
#define FEED_BACK_LEN 54

bool chassisDriver::init()
{

    if (chasis_port_->openPort() < 0)
    {
        ROS_ERROR("error : port %s open fail ", port_name_.c_str());
        return false;
    }

    /****************进程通信-消息队列*********************/
    msg_batt_id_ = msgget((key_t)msg_batt_key_, 0666 | IPC_CREAT);
    if(msg_batt_id_ == -1){// 错误处理
        fprintf(stderr, "battery msgget failed with error: %d\n", errno);
        exit(EXIT_FAILURE);
    }
    /****************进程通信-消息队列*********************/

    return true;
}

/**  msg -msg.linear.x/y -m/s;  -msg.angular.z -rad/s **/
void chassisDriver::twist_callback(const geometry_msgs::Twist::ConstPtr &msg)
{
    twist_mutex_.lock();
    current_twist_ = *msg.get();
    //最大速度限制
    if (current_twist_.linear.x > VEL_MAX){
        current_twist_.linear.x = VEL_MAX;
    } else if (current_twist_.linear.x < -VEL_MAX) {
        current_twist_.linear.x = -VEL_MAX;
    }
    if (current_twist_.linear.y > VEL_MAX){
        current_twist_.linear.y = VEL_MAX;
    } else if (current_twist_.linear.y < -VEL_MAX) {
        current_twist_.linear.y = -VEL_MAX;
    }
    if (current_twist_.angular.z > ANL_MAX){
        current_twist_.angular.z = ANL_MAX;
    } else if (current_twist_.angular.z < -ANL_MAX) {
        current_twist_.angular.z = -ANL_MAX;
    }
    twist_mutex_.unlock();
}

/////////////////
/// 消息请求函数 //
/////////////////

void chassisDriver::send_speed_callback(const ros::TimerEvent &)
{
    ChassisCtrl send_ctrl_command;
    unsigned char buff[VEL_COMMAND_LEN];

    send_ctrl_command.header1   = 0xAA;
    send_ctrl_command.header2   = 0x55;
    send_ctrl_command.length    = 0x0F;
    send_ctrl_command.sign1     = 0x12;
    send_ctrl_command.sign2     = 0x01;
    send_ctrl_command.stop_mode = 0x00;

    twist_mutex_.lock();
    send_ctrl_command.liner_speed_x = (float)current_twist_.linear.x;
    send_ctrl_command.liner_speed_y = -(float)current_twist_.linear.y;
    send_ctrl_command.angular_speed = -(float)current_twist_.angular.z;
    twist_mutex_.unlock();
    //校验位
    memcpy(buff, &send_ctrl_command, VEL_COMMAND_LEN);
    buff[VEL_COMMAND_LEN - 1] = form_sum(buff, VEL_COMMAND_LEN);

    chasis_port_->writeBytes(buff, VEL_COMMAND_LEN);
}

/////////////////
/// 工具函数  ////
/////////////////
//异或校验
unsigned char chassisDriver::form_sum(unsigned char *buf, int len)
{
    unsigned char sum = 0x00;
    for (int i = 0; i < len - 1; i++)
    {
        sum ^= buf[i];
    }
    return sum;
}

//异常处理函数
void chassisDriver::exceptionHandle(uint8_t motor, uint8_t steer, uint8_t encodisk, uint8_t collide)
{
    //电机异常
    if (motor & ERROR_R_F)
        ROS_ERROR("the motor generator of right front wheel is error !");
    if (motor & ERROR_R_M)
        ROS_ERROR("the motor generator of right middle wheel is error !");
    if (motor & ERROR_R_B)
        ROS_ERROR("the motor generator of right back wheel is error !");
    if (motor & ERROR_L_F)
        ROS_ERROR("the motor generator of left front wheel is error !");
    if (motor & ERROR_L_M)
        ROS_ERROR("the motor generator of left middle wheel is error !");
    if (motor & ERROR_L_B)
        ROS_ERROR("the motor generator of left back wheel is error !");
    //舵机驱动器异常
    if (steer & ERROR_R_F)
        ROS_ERROR("the steer gear of right front wheel is error !");
    if (steer & ERROR_R_M)
        ROS_ERROR("the steer gear of right middle wheel is error !");
    if (steer & ERROR_R_B)
        ROS_ERROR("the steer gear of right back wheel is error !");
    if (steer & ERROR_L_F)
        ROS_ERROR("the steer gear of left front wheel is error !");
    if (steer & ERROR_L_M)
        ROS_ERROR("the steer gear of left middle wheel is error !");
    if (steer & ERROR_L_B)
        ROS_ERROR("the steer gear of left back wheel is error !");
    //码盘异常
    if (encodisk & ERROR_R_F)
        ROS_ERROR("the encoding disk of right front wheel is error !");
    if (encodisk & ERROR_R_M)
        ROS_ERROR("the encoding disk of right middle wheel is error !");
    if (encodisk & ERROR_R_B)
        ROS_ERROR("the encoding disk of right back wheel is error !");
    if (encodisk & ERROR_L_F)
        ROS_ERROR("the encoding disk of left front wheel is error !");
    if (encodisk & ERROR_L_M)
        ROS_ERROR("the encoding disk of left middle wheel is error !");
    if (encodisk & ERROR_L_B)
        ROS_ERROR("the encoding disk of left back wheel is error !");
    //碰撞触发
    if (collide)
        ROS_ERROR("the collision trigger is error !");
}

void chassisDriver::parse_msg(bool isPrintMsg)
{
    int buffrxcnt = 0, count = 0;
    parse_flag_ = true;
    unsigned char data;
    unsigned char buff[FEED_BACK_LEN];
    while (parse_flag_) {
        chasis_port_->readBytes(&data, 1);
//        ROS_INFO("data : %x", data);
        if (buffrxcnt == 0) {
            if (data == 0xAA) {
                buff[buffrxcnt++] = data;
            }
        } else if (buffrxcnt == 1) {
            if (data == 0x55) {
                buff[buffrxcnt++] = data;
            }
        } else if (buffrxcnt == 2) {  //有效长度
            if (data == 0x36) {
                buff[buffrxcnt++] = data;
            }
        } else if (buffrxcnt < FEED_BACK_LEN) {
            buff[buffrxcnt++] = data;

            if (buffrxcnt == FEED_BACK_LEN) {
                if ((buff[0] == 0xAA) && (buff[1] == 0x55) &&
                    (buff[buffrxcnt - 1] == form_sum(buff, FEED_BACK_LEN))) {
//                    ROS_INFO("read a string of feedback msg successfully");
                    memcpy(&feed_back_, buff, FEED_BACK_LEN);
                    buffrxcnt = 0;
                    //１ｓ　打印一次
                    if (isPrintMsg && count >= 50) {
                        count = 0;
                        ROS_INFO("header : %x,%x", feed_back_.header1, feed_back_.header2);
                        ROS_INFO("length : %x",    feed_back_.length);
                        ROS_INFO("encode r f : %x", feed_back_.right_front_encodisk);
                        ROS_INFO("encode r m : %x", feed_back_.right_midlle_encodisk);
                        ROS_INFO("encode r b : %x", feed_back_.right_back_encodisk);
                        ROS_INFO("encode l f : %x", feed_back_.left_front_encodisk);
                        ROS_INFO("encode l m : %x", feed_back_.left_middle_encodisk);
                        ROS_INFO("encode l b : %x", feed_back_.left_back_encodisk);
                        ROS_INFO("time stamp : %d", feed_back_.time_stamp_10us);
                        ROS_INFO("wheel current r f : %d", feed_back_.right_front_current);
                        ROS_INFO("wheel current r m : %d", feed_back_.right_midlle_current);
                        ROS_INFO("wheel current r b : %d", feed_back_.right_back_current);
                        ROS_INFO("wheel current l f : %d", feed_back_.left_front_current);
                        ROS_INFO("wheel current l m : %d", feed_back_.left_midlle_current);
                        ROS_INFO("wheel current l b : %d", feed_back_.left_back_current);
                        ROS_INFO("motor err : %x",  feed_back_.motor_abnormality);
                        ROS_INFO("driver err : %x", feed_back_.drive_abnormality);
                        ROS_INFO("encoding disk err : %x", feed_back_.encode_disk_abnormality);
                        ROS_INFO("collision err : %x", feed_back_.colision_triger);
                        ROS_INFO("steer r f : %d", feed_back_.right_front_steer);
                        ROS_INFO("steer r m : %d", feed_back_.right_midlle_steer);
                        ROS_INFO("steer r b : %d", feed_back_.right_back_steer);
                        ROS_INFO("steer l f : %d", feed_back_.left_front_steer);
                        ROS_INFO("steer l m : %d", feed_back_.left_midlle_steer);
                        ROS_INFO("steer l b : %d", feed_back_.left_back_steer);
                        ROS_INFO("batter_10mV : %d", feed_back_.batter_mV);
                        ROS_INFO("charge_10mA : %d", feed_back_.charge_mA);
                        ROS_INFO("batter_percent_%% : %d", feed_back_.batter_percent);
                        ROS_INFO("Hall r f : %x", feed_back_.right_front_Hall);
                        ROS_INFO("Hall r m : %x", feed_back_.right_midlle_Hall);
                        ROS_INFO("Hall r b : %x", feed_back_.right_back_Hall);
                        ROS_INFO("Hall l f : %x", feed_back_.left_front_Hall);
                        ROS_INFO("Hall l m : %x", feed_back_.left_midlle_Hall);
                        ROS_INFO("Hall l b : %x", feed_back_.left_back_Hall);
                        ROS_INFO("check : %x", feed_back_.check);
                    }
                    if(isPrintMsg) count++;
                    // 异常处理
                    /*
                    if (feed_back_.motor_abnormality + feed_back_.drive_abnormality +
                        feed_back_.encode_disk_abnormality + feed_back_.colision_triger != 0x00) {
                        exceptionHandle(feed_back_.motor_abnormality, feed_back_.drive_abnormality,
                                        feed_back_.encode_disk_abnormality, feed_back_.colision_triger);
                        return;
                    }
                     */
                    //发送反馈信息
                    float battery_V = (float)(feed_back_.batter_mV*0.001);
                    float battery_Percent = feed_back_.batter_percent;

                    std_msgs::Float32 msg_mV, msg_percent;
                    msg_mV.data      = battery_V;
                    msg_percent.data = battery_Percent;
                    battery_V_.publish(msg_mV);
                    battery_percent_.publish(msg_percent);

                    /* 废弃方案
                    my_msg_batt_.my_msg_type = 1;
                    my_msg_batt_.mV          = battery_V;
                    my_msg_batt_.percent     = battery_Percent;
                    if(msgsnd(msg_batt_id_, (void *)&my_msg_batt_, sizeof(msg_battery_st)-sizeof(long int), 0) == -1){
                        fprintf(stderr, "battery msgsnd failed \n");
                        exit(EXIT_FAILURE);
                    }
                     */
                }
            }
        } else {
            buffrxcnt = 0;
        }
    }
}

/////////////////
/// 主执行函数
/////////////////

void chassisDriver::run()
{
    ros::NodeHandle node;
    ros::NodeHandle private_node("~");

    bool mark_print;

    private_node.param<std::string>("port_name", port_name_, std::string("/dev/ttyUSB0"));
    private_node.param<int>("baud_rate",         baud_rate_,      115200);
    private_node.param<int>("msg_battery_key",   msg_batt_key_,   1235);
    private_node.param<double>("wheel_diameter", wheel_diameter_, 0.15);
    private_node.param<bool>("mark_print_msg",   mark_print,      false);

    battery_V_       = private_node.advertise<std_msgs::Float32>("/battery_V",      1);
    battery_percent_ = private_node.advertise<std_msgs::Float32>("/battry_percent", 1);

    chasis_port_ = new SerialPort(port_name_.c_str(), baud_rate_);

    if (init())
    {
        ros::Subscriber cmd_sub = node.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, &chassisDriver::twist_callback, this);
        ros::Timer send_speed_timer = node.createTimer(ros::Duration(1.0 / 50), &chassisDriver::send_speed_callback, this);

        parse_thread_ = boost::thread(boost::bind(&chassisDriver::parse_msg, this, mark_print));

        ros::spin();
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "chassis_driver");
    chassisDriver driver;
    driver.run();

    return 0;
}